// Psoden's Aviaduino
// March 2009
// v001

// Enable << stream with print
//template inline Print &operator <<(Print &obj, T arg)
//{ obj.print(arg); return obj; }

#include <Messenger.h>
#include <ServoTimer2.h>
#include <EEPROM.h>

/*************** Initialize variables ***************/

// XBee Communication
#define BAUD 9600
Messenger xbee = Messenger();

// Define sensor pin assignments
// - Gyro
#define ROLLRATEPIN 4
#define PITCHRATEPIN 3
#define YAWRATEPIN 5

// PID Values
#define WINDUP_GUARD_GAIN 100.0
#define PGAIN_ADR 0
#define IGAIN_ADR 4
#define DGAIN_ADR 8
float pTerm, iTerm, dTerm; 
float pgain = 0;
float igain = 0;
float dgain = 0;





/*************** Setup Aviaduino ***************/
void setup() {
  Serial.begin(BAUD); 
  
  // Read PID values form EEPROM
  pgain = readFloat(PGAIN_ADR);
  igain = readFloat(IGAIN_ADR);
  dgain = readFloat(DGAIN_ADR);
  
  // Attach the callback function to the Messenger
  xbee.attach(messageReady);
}


void loop() {
  // The following line is the most effective way of using Serial and Messenger's callback
  while ( Serial.available() )  xbee.process(Serial.read () );
}
